#include "stdafx.h"
#include "gp_Trsf2d.h"
#include "gp_Trsf2d.h"
#include "gp_Mat2d.h"
#include "gp_XY.h"
#include "gp_Ax2d.h"
#include "gp_GTrsf2d.h"
#include "gp.h"

void gp_GTrsf2d::SetAffinity (const gp_Ax2d& A,
	const double Ratio)
{
	shape = gp_Other;
	scale = 0.0;
	double a = A.Direction().X();
	double b = A.Direction().Y();
	matrix.SetValue (1, 1, (1.0 - Ratio)*a*a + Ratio);
	matrix.SetValue (2, 2, (1.0 - Ratio)*b*b + Ratio);
	matrix.SetValue (1, 2, (1.0 - Ratio)*a*b);
	matrix.SetValue (2, 1, matrix.Value (1, 2));
	loc = A.Location().XY();
	loc.Reverse();
	loc.Multiply (matrix);
	loc.Add (A.Location().XY());
}

void gp_GTrsf2d::SetTranslationPart (const gp_XY& Coord)
{
	loc = Coord;
	if (Form() == gp_CompoundTrsf || Form() == gp_Other || 
		Form() == gp_Translation) { }
	else if (Form() == gp_Identity) { shape = gp_Translation; }
	else { shape = gp_CompoundTrsf; }
}

void gp_GTrsf2d::Invert ()
{
	if (shape == gp_Other) {
		matrix.Invert();
		loc.Multiply (matrix);
		loc.Reverse();
	}
	else {
		gp_Trsf2d T = Trsf2d ();
		T.Invert ();
		SetTrsf2d (T);
	}
}

void gp_GTrsf2d::Multiply (const gp_GTrsf2d& T)
{
	if (Form() == gp_Other || T.Form() == gp_Other) {
		shape = gp_Other;
		loc.Add (T.loc.Multiplied (matrix));
		matrix.Multiply(T.matrix);
	}
	else {
		gp_Trsf2d T1 = Trsf2d();
		gp_Trsf2d T2 = T.Trsf2d();
		T1.Multiply(T2);
		matrix = T1.matrix;
		loc = T1.loc;
		scale = T1.scale;
		shape = T1.shape;
	}
}

void gp_GTrsf2d::Power (const int N)
{
	if (N == 0)  {
		scale = 1.0;
		shape = gp_Identity;
		matrix.SetIdentity();
		loc = gp_XY (0., 0.);
	}
	else if (N == 1) { }
	else if (N == -1) { Invert(); }
	else {
		if (N < 0) { Invert(); }
		if (shape == gp_Other) {
			int Npower = N;
			if (Npower < 0) Npower = - Npower;
			Npower--;
			gp_XY Temploc = loc;
			//      double Tempscale = scale;
			gp_Mat2d Tempmatrix (matrix);
			for(;;) {
				if (IsOdd(Npower)) {
					loc.Add (Temploc.Multiplied (matrix));
					matrix.Multiply (Tempmatrix);
				}
				if (Npower == 1) { break; }
				Temploc.Add (Temploc.Multiplied (Tempmatrix));
				Tempmatrix.Multiply (Tempmatrix);
				Npower = Npower/2;
			}
		}
		else {
			gp_Trsf2d T = Trsf2d ();
			T.Power (N);
			SetTrsf2d (T);
		}
	}
}

void gp_GTrsf2d::PreMultiply (const gp_GTrsf2d& T)
{
	if (Form() == gp_Other || T.Form() == gp_Other) {
		shape = gp_Other;
		loc.Multiply (T.matrix);
		loc.Add (T.loc);
		matrix.PreMultiply(T.matrix);
	}
	else {
		gp_Trsf2d T1 = Trsf2d();
		gp_Trsf2d T2 = T.Trsf2d();
		T1.PreMultiply(T2);
		matrix = T1.matrix;
		loc = T1.loc;
		scale = T1.scale;
		shape = T1.shape;
	}
}

gp_Trsf2d gp_GTrsf2d::Trsf2d () const
{
	gp_Trsf2d T;
	double value;
	double tolerance = Precision_Angular;
	double tol2 = 2. * tolerance;


	Standard_ConstructionError_Raise_if
		(Form() == gp_Other," gp_GTrsf2d::Trsf2d() - non-orthogonal GTrsf2d (0)");


	//Test of orthogonality

	value = (matrix.Value(1,1) * matrix.Value(1,1) +
		matrix.Value(2,1) * matrix.Value(2,1)) ;
	Standard_ConstructionError_Raise_if
		(RealAbs(value - 1.) > tol2," gp_GTrsf2d::Trsf2d() - non-orthogonal GTrsf2d (1)");

	value = (matrix.Value(1,2) * matrix.Value(1,2) +
		matrix.Value(2,2) * matrix.Value(2,2));
	Standard_ConstructionError_Raise_if
		(RealAbs(value - 1.) > tol2," gp_GTrsf2d::Trsf2d() - non-orthogonal GTrsf2d (2)");

	value = (matrix.Value(1,1) * matrix.Value(1,2) +
		matrix.Value(2,1) * matrix.Value(2,2));
	Standard_ConstructionError_Raise_if
		(RealAbs(value) > tolerance," gp_GTrsf2d::Trsf2d() - non-orthogonal GTrsf2d (3)");
	//

	T.matrix = matrix ;
	T.shape = shape;
	T.scale = scale ;
	T.loc = loc;

	return T;
}

 gp_GTrsf2d::gp_GTrsf2d ()
{
  shape = gp_Identity;
  matrix.SetScale (1.0);
  loc.SetCoord (0.0, 0.0);
  scale = 1.0;
}

 gp_GTrsf2d::gp_GTrsf2d (const gp_Trsf2d& T)
{
  shape = T.shape;
  matrix = T.matrix;
  loc = T.loc;
  scale = T.scale;
}

 gp_GTrsf2d::gp_GTrsf2d (const gp_Mat2d& M,
			       const gp_XY& V) :
			       matrix(M),
			       loc(V)
{ shape = gp_Other;  scale = 0.0; }

 void gp_GTrsf2d::SetValue (const int Row,
				  const int Col,
				  const double Value)
{
  Standard_OutOfRange_Raise_if
    (Row < 1 || Row > 2 || Col < 1 || Col > 3, " ");
  if (Col == 3) loc.SetCoord (Row, Value);
  else matrix.SetValue (Row, Col, Value);
  shape = gp_Other;
}

 void gp_GTrsf2d::SetTrsf2d (const gp_Trsf2d& T)
{
  shape = T.shape;
  matrix = T.matrix;
  loc = T.loc;
  scale = T.scale;
}

 void gp_GTrsf2d::SetVectorialPart (const gp_Mat2d& Matrix)
{ matrix = Matrix;   shape = gp_Other;  scale = 0.0; }

 bool gp_GTrsf2d::IsNegative () const
{ return matrix.Determinant() < 0.0; }

 bool gp_GTrsf2d::IsSingular () const
{ return matrix.IsSingular(); }

 const gp_Mat2d& gp_GTrsf2d::VectorialPart () const
{ return matrix; }

 double gp_GTrsf2d::Value (const int Row,
					const int Col) const
{
  Standard_OutOfRange_Raise_if
    (Row < 1 || Row > 2 || Col < 1 || Col > 3, " ");
  if (Col == 3) return loc.Coord (Row); 
  if (shape == gp_Other) return matrix.Value (Row, Col);
  return scale * matrix.Value (Row, Col);
}

 gp_TrsfForm gp_GTrsf2d::Form () const
{ return shape; }

 const gp_XY& gp_GTrsf2d::TranslationPart () const
{ return loc; }

 gp_GTrsf2d gp_GTrsf2d::Inverted () const
{
  gp_GTrsf2d T = *this;
  T.Invert ();
  return T;
}

 gp_GTrsf2d gp_GTrsf2d::Multiplied (const gp_GTrsf2d& T) const
{
  gp_GTrsf2d Tres = *this;
  Tres.Multiply (T);
  return Tres;
}

 gp_GTrsf2d gp_GTrsf2d::Powered (const int N) const
{
  gp_GTrsf2d T = *this;
  T.Power (N);
  return T;
}

 void gp_GTrsf2d::Transforms (gp_XY& Coord) const
{
  Coord.Multiply (matrix);
  if (!(shape == gp_Other) && !(scale == 1.0)) Coord.Multiply (scale);
  Coord.Add(loc);
}

 gp_XY gp_GTrsf2d::Transformed(const gp_XY& Coord) const 
{
  gp_XY newCoord = Coord;
  Transforms(newCoord);
  return newCoord;
}

 void gp_GTrsf2d::Transforms (double& X,
				    double& Y) const
{
  gp_XY Doublet (X, Y);
  Doublet.Multiply (matrix);
  if (!(shape == gp_Other) && !(scale == 1.0)) Doublet.Multiply (scale);
  Doublet.Add(loc);
  Doublet.Coord (X, Y);
}

